﻿using ROSBridgeLib;
using ROSBridgeLib.sensor_msgs;
using ROSBridgeLib.std_msgs;
using ROSBridgeLib.geometry_msgs;
using ROSBridgeLib.nav_msgs;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using SimpleJSON;

public class TFSubscriber : BaseSubscriber
{

	
    public Frame[] frames;

    private Hashtable frameTransforms;


    void Start()
    {

        topic = "tf";

        messageType = "tf/tfMessage";

        frameTransforms = new Hashtable();

        frames = FindObjectsOfType<Frame>() as Frame[];

        foreach (Frame frame in frames)
        {

            frameTransforms.Add(frame.frame_id, frame.transform);

        }



    }



    public override ROSBridgeMsg Parse(JSONNode msg)
    {
        return new TransformsMsg(msg["msg"]);
    }
    public override void OnMessage()
    {


        TransformsMsg transformMsg=(TransformsMsg)GetTopParsedPackage();

        if (transformMsg==null)
        {
            return;

        }


        UpdateTransform(transformMsg);

    }


    void UpdateTransform(TransformsMsg transforms)
    {


        foreach (TransformMsg tran in transforms.transforms)
        {


            if (frameTransforms==null)
            {
                continue;
            }

            Transform parent = (Transform)frameTransforms[tran.frameId];

            Transform child = frameTransforms[tran.childFrameId] as Transform;


            if (parent != null && child != null)
            {


                Vector3 nextPosition = parent.TransformPoint(tran.translation.ToVector3());
                    
                Quaternion nextRotation = parent.rotation * tran.rotation.ToQuaternion();

                child.position=nextPosition;

                child.rotation=nextRotation;


            }


        }



    }

}
